﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;

/// <summary>
/// 平滑路径曲线
/// </summary>

[DisallowMultipleComponent]
public class SmoothPathCurve : MonoBehaviour
{
    [Header("是否循环")]
    [Tooltip("曲面循环的话，最少需要5个控制节点才不会出现撕裂")]
    public bool m_Looped;
    public bool Looped { get { return m_Looped; } }
    [Header("路径采样率")]
    [Range(1, 100)]
    public int m_Resolution = 20;
    public int DistanceCacheSampleStepsPerSegment { get { return m_Resolution; } }
    [Serializable]
    public class Appearance
    {
        [Tooltip("路径对象选中时颜色")]
        public Color pathColor = Color.green;
        [Tooltip("未选中时颜色")]
        public Color inactivePathColor = Color.gray;
        [Tooltip("路径宽度")]
        [Range(0f, 10f)]
        public float width = 0.2f;
    }
    [Tooltip("控制路径在编辑器场景视图中的显示方式的设置。")]
    public Appearance m_Appearance = new Appearance();

    /// <summary>
    /// 路径控制点
    /// </summary>
    [Serializable]
    public struct Waypoint
    {
        [Tooltip("路径点，本地坐标")]
        public Vector3 position;
        [Tooltip("路径点旋转角度")]
        public float roll;

        internal Vector4 AsVector4
        {
            get { return new Vector4(position.x, position.y, position.z, roll); }
        }

        internal static Waypoint FromVector4(Vector4 v)
        {
            Waypoint wp = new Waypoint();
            wp.position = new Vector3(v.x, v.y, v.z);
            wp.roll = v.w;
            return wp;
        }

    }

    public Waypoint[] m_Waypoints = new Waypoint[0];

    public float MinPos { get { return 0; } }
    public float MaxPos {
        get
        {
            int count = m_Waypoints.Length - 1;
            if (count < 1) return 0;
            return m_Looped ? count + 1 : count;
        }
    }

    /// <summary>
    /// 以距离为单位求路径的长度。
    /// </summary>
    public float PathLength
    {
        get
        {
            if (DistanceCacheSampleStepsPerSegment < 1)
                return 0;
            if (!DistanceCacheIsValid())
                ResamplePath(DistanceCacheSampleStepsPerSegment);
            return m_PathLength;
        }
    }

    /// <summary>How to interpret the Path Position</summary>
    public enum PositionUnits
    {
        /// <summary>U使用PathPosition单元，其中0是第一个路径点，1是第二个路径点</summary>
        PathUnits,
        /// <summary>使用沿路径的距离。路径将根据其分辨率进行采样设置时，将在内部缓存一个距离查找表</summary>
        Distance,
        /// <summary>标准化单位，0是路径的起点，1是终点。</summary>
        Normalized
    }



    private float[] m_DistanceToPos;
    private float[] m_PosToDistance;
    private int m_CachedSampleSteps;
    private float m_PathLength;
    private float m_cachedPosStepSize;
    private float m_cachedDistanceStepSize;
    Waypoint[] m_ControlPoints1;
    Waypoint[] m_ControlPoints2;
    bool m_IsLoopedCache;


#if UNITY_EDITOR
    protected virtual void OnValidate()
    {
        InvalidateDistanceCache();
    }

#endif
    /// <summary>
    /// 重置按键，Mono脚本自动加载，editor下使用
    /// </summary>

    protected virtual void Reset()
    {
        InvalidateDistanceCache();
        m_Looped = false;
        m_Waypoints = new Waypoint[2]
        {
            new Waypoint{position = new Vector3(0,0,-5)},
            new Waypoint{position = new Vector3(0,0,5)}
        };
        m_Appearance = new Appearance();
    }

    public void InvalidateDistanceCache()
    {
        m_DistanceToPos = null;
        m_PosToDistance = null;
        m_CachedSampleSteps = 0;
        m_PathLength = 0;
        m_ControlPoints1 = null;
        m_ControlPoints2 = null;
    }
    public bool DistanceCacheIsValid()
    {
        return (MaxPos == MinPos)
            || (m_DistanceToPos != null && m_PosToDistance != null
                && m_CachedSampleSteps == DistanceCacheSampleStepsPerSegment
                && m_CachedSampleSteps > 0);
    }

    public virtual void UpdatePoint(int index, Vector3 pos)
    {
        if(index< m_Waypoints.Length) m_Waypoints[index].position = pos;
    }

    public Vector3 EvaluateLocalPosition(float pos)
    {
        Vector3 result = Vector3.zero;
        if (m_Waypoints.Length > 0)
        {
            UpdateControlPoints();
            int indexA, indexB;
            pos = GetBoundingIndices(pos, out indexA, out indexB);
            if (indexA == indexB)
                result = m_Waypoints[indexA].position;
            else
                result = CurveUtily.Bezier3(pos - indexA, m_Waypoints[indexA].position, m_ControlPoints1[indexA].position,
                        m_ControlPoints2[indexA].position, m_Waypoints[indexB].position);
        }
        return result;
    }

    public Vector3 EvaluatePosition(float pos)
    {
        Vector3 result = Vector3.zero;
        if(m_Waypoints.Length > 0)
        {
            UpdateControlPoints();
            int indexA, indexB;
            pos = GetBoundingIndices(pos, out indexA, out indexB);
            if (indexA == indexB)
                result = m_Waypoints[indexA].position;
            else
                result = CurveUtily.Bezier3(pos - indexA, m_Waypoints[indexA].position, m_ControlPoints1[indexA].position,
                        m_ControlPoints2[indexA].position, m_Waypoints[indexB].position);
        }
        return transform.TransformPoint(result);
    }

    public Vector3 EvaluateTangent(float pos)
    {
        Vector3 result = transform.rotation * Vector3.forward;
        if (m_Waypoints.Length > 1)
        {
            UpdateControlPoints();
            int indexA, indexB;
            pos = GetBoundingIndices(pos, out indexA, out indexB);
            if (!Looped && indexA == m_Waypoints.Length - 1) --indexA;

            result = CurveUtily.BezierTangent3(pos - indexA, m_Waypoints[indexA].position, m_ControlPoints1[indexA].position, m_ControlPoints2[indexA].position, m_Waypoints[indexB].position);
        }
        return transform.TransformDirection(result);
    }

    public Quaternion EvaluateOrientation(float pos)
    {
        Quaternion transformRot = transform.rotation;
        Vector3 transformUp = transformRot * Vector3.up;
        Quaternion result = transformRot;
        if (m_Waypoints.Length > 0)
        {
            float roll = 0;
            int indexA, indexB;
            pos = GetBoundingIndices(pos, out indexA, out indexB);
            if (indexA == indexB)
                roll = m_Waypoints[indexA].roll;
            else
            {
                UpdateControlPoints();
                roll = CurveUtily.Bezier1(pos - indexA,
                    m_Waypoints[indexA].roll, m_ControlPoints1[indexA].roll,
                    m_ControlPoints2[indexA].roll, m_Waypoints[indexB].roll);
            }

            Vector3 fwd = EvaluateTangent(pos);
            if (!Mathf.Approximately(fwd.magnitude,0.0f))
            {
                Quaternion q = Quaternion.LookRotation(fwd, transformUp);
                result = q * RollAroundForward(roll);
            }
        }
        return result;
    }

    // same as Quaternion.AngleAxis(roll, Vector3.forward), just simplified
    Quaternion RollAroundForward(float angle)
    {
        float halfAngle = angle * 0.5F * Mathf.Deg2Rad;
        return new Quaternion(
            0,
            0,
            Mathf.Sin(halfAngle),
            Mathf.Cos(halfAngle));
    }

    void UpdateControlPoints()
    {
        int numPoints = m_Waypoints == null ? 0 : m_Waypoints.Length;
        if (numPoints > 1 && (Looped != m_IsLoopedCache || m_ControlPoints1 == null || m_ControlPoints1.Length != numPoints
            || m_ControlPoints2 == null || m_ControlPoints2.Length != numPoints))
        {
            Vector4[] p1 = new Vector4[numPoints];
            Vector4[] p2 = new Vector4[numPoints];
            Vector4[] K = new Vector4[numPoints];
            for(int i = 0; i < numPoints; ++i)
            {
                K[i] = m_Waypoints[i].AsVector4;
            }
            if (Looped)
                CurveUtily.ComputeSmoothControlPointsLooped(ref K, ref p1, ref p2); 
            else
                CurveUtily.ComputeSmoothControlPoints(ref K,ref p1,ref p2);
            m_ControlPoints1 = new Waypoint[numPoints];
            m_ControlPoints2 = new Waypoint[numPoints];
            for(int i = 0; i < numPoints; ++i)
            {
                m_ControlPoints1[i] = Waypoint.FromVector4(p1[i]);
                m_ControlPoints2[i] = Waypoint.FromVector4(p2[i]);
            }
            m_IsLoopedCache = Looped;
        }
    }
   
    float GetBoundingIndices(float pos,out int indexA,out int indexB)
    {
        pos = StandardizePos(pos);
        int numWaypoints = m_Waypoints.Length;
        if (numWaypoints < 2) indexA = indexB = 0;
        else
        {
            indexA = Mathf.FloorToInt(pos);
            if(indexA >= numWaypoints)
            {
                // Only true if looped
                pos -= MaxPos;
                indexA = 0;
            }
            indexB = indexA + 1;
            if(indexB == numWaypoints)
            {
                if (Looped) indexB = 0;
                else
                {
                    --indexB;
                    --indexA;
                }
            }
        }
        return pos;
    }

    /// <summary>
    /// 得到一个标准化的路径位置，如果绕圈，考虑旋转
    /// </summary>
    /// <param name="pos"></param>
    /// <returns></returns>
    float StandardizePos(float pos)
    {
        if(Looped && MaxPos > 0)
        {
            pos = pos % MaxPos;
            if (pos < 0) pos += MaxPos;
            return pos;
        }
        return Mathf.Clamp(pos, 0, MaxPos);
    }

    private void ResamplePath(int stepsPerSegment)
    {
        InvalidateDistanceCache();

        float minPos = MinPos;
        float maxPos = MaxPos;
        float stepSize = 1f / Mathf.Max(1, stepsPerSegment);

        // Sample the positions
        int numKeys = Mathf.RoundToInt((maxPos - minPos) / stepSize) + 1;
        m_PosToDistance = new float[numKeys];
        m_CachedSampleSteps = stepsPerSegment;
        m_cachedPosStepSize = stepSize;

        Vector3 p0 = EvaluatePosition(0);
        m_PosToDistance[0] = 0;
        float pos = minPos;
        for (int i = 1; i < numKeys; ++i)
        {
            pos += stepSize;
            Vector3 p = EvaluatePosition(pos);
            float d = Vector3.Distance(p0, p);
            m_PathLength += d;
            p0 = p;
            m_PosToDistance[i] = m_PathLength;
        }

        // Resample the distances
        m_DistanceToPos = new float[numKeys];
        m_DistanceToPos[0] = 0;
        if (numKeys > 1)
        {
            stepSize = m_PathLength / (numKeys - 1);
            m_cachedDistanceStepSize = stepSize;
            float distance = 0;
            int posIndex = 1;
            for (int i = 1; i < numKeys; ++i)
            {
                distance += stepSize;
                float d = m_PosToDistance[posIndex];
                while (d < distance && posIndex < numKeys - 1)
                    d = m_PosToDistance[++posIndex];
                float d0 = m_PosToDistance[posIndex - 1];
                float delta = d - d0;
                float t = (distance - d0) / delta;
                m_DistanceToPos[i] = m_cachedPosStepSize * (t + posIndex - 1);
            }
        }
    }

    /// <summary>Get a worldspace position of a point along the path</summary>
    /// <param name="pos">Postion along the path.  Need not be normalized.</param>
    /// <param name="units">The unit to use when interpreting the value of pos.</param>
    /// <returns>World-space position of the point along at path at pos</returns>
    public Vector3 EvaluatePositionAtUnit(float pos, PositionUnits units)
    {
        return EvaluatePosition(ToNativePathUnits(pos, units));
    }

    /// <summary>Get the tangent of the curve at a point along the path.</summary>
    /// <param name="pos">Postion along the path.  Need not be normalized.</param>
    /// <param name="units">The unit to use when interpreting the value of pos.</param>
    /// <returns>World-space direction of the path tangent.
    /// Length of the vector represents the tangent strength</returns>
    public Vector3 EvaluateTangentAtUnit(float pos, PositionUnits units)
    {
        return EvaluateTangent(ToNativePathUnits(pos, units));
    }

    /// <summary>Get the orientation the curve at a point along the path.</summary>
    /// <param name="pos">Postion along the path.  Need not be normalized.</param>
    /// <param name="units">The unit to use when interpreting the value of pos.</param>
    /// <returns>World-space orientation of the path</returns>
    public Quaternion EvaluateOrientationAtUnit(float pos, PositionUnits units)
    {
        return EvaluateOrientation(ToNativePathUnits(pos, units));
    }


    /// <summary>Get the minimum value, for the given unit type</summary>
    /// <param name="units">The unit type</param>
    /// <returns>The minimum allowable value for this path</returns>
    public float MinUnit(PositionUnits units)
    {
        if (units == PositionUnits.Normalized)
            return 0;
        return units == PositionUnits.Distance ? 0 : MinPos;
    }

    /// <summary>Get the maximum value, for the given unit type</summary>
    /// <param name="units">The unit type</param>
    /// <returns>The maximum allowable value for this path</returns>
    public float MaxUnit(PositionUnits units)
    {
        if (units == PositionUnits.Normalized)
            return 1;
        return units == PositionUnits.Distance ? PathLength : MaxPos;
    }

    /// <summary>Standardize the unit, so that it lies between MinUmit and MaxUnit</summary>
    /// <param name="pos">The value to be standardized</param>
    /// <param name="units">The unit type</param>
    /// <returns>The standardized value of pos, between MinUnit and MaxUnit</returns>
    public float StandardizeUnit(float pos, PositionUnits units)
    {
        if (units == PositionUnits.PathUnits)
            return StandardizePos(pos);
        if (units == PositionUnits.Distance)
            return StandardizePathDistance(pos);
        float len = PathLength;
        if (len < 0.0001f)
            return 0;
        return StandardizePathDistance(pos * len) / len;
    }

    /// <summary>Standardize a distance along the path based on the path length.
    /// If the distance cache is not valid, then calling this will
    /// trigger a potentially costly regeneration of the path distance cache</summary>
    /// <param name="distance">The distance to standardize</param>
    /// <returns>The standardized distance, ranging from 0 to path length</returns>
    public float StandardizePathDistance(float distance)
    {
        float length = PathLength;
        if (length < Vector3.kEpsilon)
            return 0;
        if (Looped)
        {
            distance = distance % length;
            if (distance < 0)
                distance += length;
        }
        return Mathf.Clamp(distance, 0, length);
    }

    /// <summary>Get the path position to native path units.
    /// If the distance cache is not valid, then calling this will
    /// trigger a potentially costly regeneration of the path distance cache</summary>
    /// <param name="pos">The value to convert from</param>
    /// <param name="units">The units in which pos is expressed</param>
    /// <returns>The path position, in native units</returns>
    public float ToNativePathUnits(float pos, PositionUnits units)
    {
        if (units == PositionUnits.PathUnits)
            return pos;
        if (DistanceCacheSampleStepsPerSegment < 1 || PathLength < 0.0001f)
            return MinPos;
        if (units == PositionUnits.Normalized)
            pos *= PathLength;
        pos = StandardizePathDistance(pos);
        float d = pos / m_cachedDistanceStepSize;
        int i = Mathf.FloorToInt(d);
        if (i >= m_DistanceToPos.Length - 1)
            return MaxPos;
        float t = d - (float)i;
        return MinPos + Mathf.Lerp(m_DistanceToPos[i], m_DistanceToPos[i + 1], t);
    }

    /// <summary>Convert a path position from native path units to the desired units.
    /// If the distance cache is not valid, then calling this will
    /// trigger a potentially costly regeneration of the path distance cache</summary>
    /// <param name="pos">The value to convert from, in native units</param>
    /// <param name="units">The units to convert to</param>
    /// <returns>The path position, in the requested units</returns>
    public float FromPathNativeUnits(float pos, PositionUnits units)
    {
        if (units == PositionUnits.PathUnits)
            return pos;
        float length = PathLength;
        if (DistanceCacheSampleStepsPerSegment < 1 || length < 0.0001f)
            return 0;
        pos = StandardizePos(pos);
        float d = pos / m_cachedPosStepSize;
        int i = Mathf.FloorToInt(d);
        if (i >= m_PosToDistance.Length - 1)
            pos = m_PathLength;
        else
        {
            float t = d - (float)i;
            pos = Mathf.Lerp(m_PosToDistance[i], m_PosToDistance[i + 1], t);
        }
        if (units == PositionUnits.Normalized)
            pos /= length;
        return pos;
    }
}
